﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace tamzarian
{
    public static class QuaternionTransformation
    {
        public static Vector3 Transfom(this Quaternion q, Vector3 v)
        {
            var WX = q.W*q.X;
            var WY = q.W*q.Y;
            var WZ = q.W*q.Z;
            var XX = q.X*q.X;
            var XY = q.X*q.Y;
            var XZ = q.X*q.Z;
            var YY = q.Y*q.Y;
            var YZ = q.Y*q.Z;
            var ZZ = q.Z*q.Z;
            return new Vector3(
             2*( (-YY - ZZ)*v.X + (+XY - WZ)*v.Y + (+WY + XZ)*v.Z ) + v.X,
             2*( (+WZ + XY)*v.X + (-XX - ZZ)*v.Y + (+YZ - WX)*v.Z ) + v.Y,
             2*( (+XZ - WY)*v.X + (+WX + YZ)*v.Y + (-XX - YY)*v.Z ) + v.Z);
        }
    }
}
